﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using starLiGHT.Physic.TwoD;
using Microsoft.Xna.Framework;
using starLiGHT.Collections;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************
#endregion

namespace starLiGHT.Physic
{
    public class PhysicSystem2D
    {
        #region Singleton
        static PhysicSystem2D instance;
        public static PhysicSystem2D Instance { get { if (instance == null) instance = new PhysicSystem2D(); return instance; } }
        #endregion

        #region Member Variables
        World world;
        FastList<Body> bodiesToDestroy;
        FastList<Joint> jointsToDestroy;
        FastList<RigidBody2D> managedBodies;
        FastList<Controller2D> controllers;
        internal FastList<Joint2D> managedJoints;
        internal int enabledControllers;

        #endregion

        #region PRIVATE Constructors
        private PhysicSystem2D()
        {
            bodiesToDestroy = new FastList<Body>();
            jointsToDestroy = new FastList<Joint>();
            managedBodies = new FastList<RigidBody2D>();
            managedJoints = new FastList<Joint2D>();
        }

        #endregion

        #region Properties
        public World World
        {
            get { return world; }
            set
            {
                if (world == value) return;

                if (world != null)
                {
                    world.WorldUnlock -= world_WorldUnlock;
                    world = null;
                }

                world = value;
                if (world != null)
                    world.WorldUnlock += new EventHandler(world_WorldUnlock);
            }
        }

        public IEnumerable<Body> BodyList
        {
            get
            {
                for (Body b = world.GetBodyList(); b != null; b = b.m_next)
                {
                    yield return b;
                }
            }
        }

        public IEnumerable<RigidBody2D> RigidBodyList
        {
            get
            {
                foreach (RigidBody2D rb in managedBodies)
                {
                    yield return rb;
                }
            }
        }

        public IEnumerable<Joint> JointList
        {
            get
            {
                for (Joint j = world.GetJointList(); j != null; j = j.m_next)
                {
                    yield return j;
                }
            }
        }

        public IEnumerable<Joint2D> Joint2DList
        {
            get
            {
                foreach (Joint2D j in managedJoints)
                {
                    yield return j;
                }
            }
        }

        #endregion

        #region Methods

        #region createBox
        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(float x, float y, float width, float height, float friction) { return createBox(BodyType.DYNAMIC, new Vector2(x, y), width, height, 0.0f, Vector2.Zero, 0.0f, friction, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(float x, float y, float width, float height) { return createBox(BodyType.DYNAMIC, new Vector2(x, y), width, height, 0.0f, Vector2.Zero, 0.0f, 0.2f, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(float x, float y, Vector2 size, float friction) { return createBox(BodyType.DYNAMIC, new Vector2(x, y), size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, friction, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(float x, float y, Vector2 size) { return createBox(BodyType.DYNAMIC, new Vector2(x, y), size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, 0.2f, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(float x, float y, Vector2 size, float friction, float density) { return createBox(BodyType.DYNAMIC, new Vector2(x, y), size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(float x, float y, Vector2 size, float angle, float friction, float density) { return createBox(BodyType.DYNAMIC, new Vector2(x, y), size.X, size.Y, angle, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(float x, float y, float width, float height, float friction, float density) { return createBox(BodyType.DYNAMIC, new Vector2(x, y), width, height, 0.0f, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(float x, float y, float width, float height, float angle, float friction, float density) { return createBox(BodyType.DYNAMIC, new Vector2(x, y), width, height, angle, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, float x, float y, float width, float height, float friction) { return createBox(type, new Vector2(x, y), width, height, 0.0f, Vector2.Zero, 0.0f, friction, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, float x, float y, float width, float height) { return createBox(type, new Vector2(x, y), width, height, 0.0f, Vector2.Zero, 0.0f, 0.2f, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, float x, float y, Vector2 size, float friction) { return createBox(type, new Vector2(x, y), size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, friction, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, float x, float y, Vector2 size) { return createBox(type, new Vector2(x, y), size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, 0.2f, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, float x, float y, Vector2 size, float friction, float density) { return createBox(type, new Vector2(x, y), size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, float x, float y, Vector2 size, float angle, float friction, float density) { return createBox(type, new Vector2(x, y), size.X, size.Y, angle, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, float x, float y, float width, float height, float friction, float density) { return createBox(type, new Vector2(x, y), width, height, 0.0f, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, float x, float y, float width, float height, float angle, float friction, float density) { return createBox(type, new Vector2(x, y), width, height, angle, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(Vector2 position, float width, float height, float friction) { return createBox(BodyType.DYNAMIC, position, width, height, 0.0f, Vector2.Zero, 0.0f, friction, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(Vector2 position, float width, float height) { return createBox(BodyType.DYNAMIC, position, width, height, 0.0f, Vector2.Zero, 0.0f, 0.2f, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(Vector2 position, Vector2 size, float friction) { return createBox(BodyType.DYNAMIC, position, size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, friction, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(Vector2 position, Vector2 size) { return createBox(BodyType.DYNAMIC, position, size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, 0.2f, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(Vector2 position, Vector2 size, float friction, float density) { return createBox(BodyType.DYNAMIC, position, size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        public RigidBody2D createBox(Vector2 position, float width, float height, float friction, float density) { return createBox(BodyType.DYNAMIC, position, width, height, 0.0f, Vector2.Zero, 0.0f, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, Vector2 position, float width, float height, float friction) { return createBox(type, position, width, height, 0.0f, Vector2.Zero, 0.0f, friction, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, Vector2 position, float width, float height) { return createBox(type, position, width, height, 0.0f, Vector2.Zero, 0.0f, 0.2f, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, Vector2 position, Vector2 size, float friction) { return createBox(type, position, size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, friction, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, Vector2 position, Vector2 size) { return createBox(type, position, size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, 0.2f, 1.0f); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, Vector2 position, Vector2 size, float friction, float density) { return createBox(type, position, size.X, size.Y, 0.0f, Vector2.Zero, 0.0f, friction, density); }

        public RigidBody2D createBox(BodyType type, Vector2 position, Vector2 size, Vector2 center, float origin, float friction, float density) { return createBox(type, position, size.X, size.Y, 0.0f, center, origin, friction, density); }

        /// <summary>
        /// Creates a rigid body with a rectangular shape. <br/>
        /// Bodies have position and velocity. You can apply forces, torques and impulses to bodies. 
        /// </summary>
        /// <param name="type">
        /// STATIC    - a static body does not move and behaves as if it has infinite mass. Static bodies can 
        ///             be moved manually by the user. A static body has always zero velocity and never collides 
        ///             with static or kinematic bodies.<br/>
        /// KINEMATIC - a kinematic body moves accoding to its velocity and never responses to forces. Normally 
        ///             kinematic bodies are moved by setting its velocity but you can move them manually. They never 
        ///             collide with other kinematic or static bodies.
        /// DYNAMIC   - is a dynamic body that is fully simulated by the engine and are moving according to its forces. It
        ///             will collide with all other body types and always have a finite, non-zero mass.
        /// </param>
        public RigidBody2D createBox(BodyType type, Vector2 position, float width, float height, float angle, Vector2 center, float orientation, float friction, float density)
        {
            PolygonShape shape = new PolygonShape();
            shape.SetAsBox(width, height, center, orientation);

            FixtureDef fd = new FixtureDef();
            fd.shape = shape;
            fd.density = density;
            fd.friction = friction;

            BodyDef bd = new BodyDef();
            bd.type = type;
            bd.position = position;
            bd.angle = angle;

            Body body = world.CreateBody(bd);
            body.CreateFixture(fd);

            RigidBody2D rb = new RigidBody2D(body);
            managedBodies.Add(rb);
            return rb;
        }

#endregion

        #region createPolygon
        public RigidBody2D createPolygon(float x, float y, float friction, params Vector2[] vertices) { return createPolygon(BodyType.DYNAMIC, new Vector2(x, y), Vector2.Zero, 0.0f, friction, 1.0f, vertices); }
        public RigidBody2D createPolygon(float x, float y, params Vector2[] vertices) { return createPolygon(BodyType.DYNAMIC, new Vector2(x, y), Vector2.Zero, 0.0f, 0.2f, 1.0f, vertices); }
        public RigidBody2D createPolygon(float x, float y, float friction, float density, params Vector2[] vertices) { return createPolygon(BodyType.DYNAMIC, new Vector2(x, y), Vector2.Zero, 0.0f, friction, density, vertices); }
        public RigidBody2D createPolygon(BodyType type, float x, float y, float friction, params Vector2[] vertices) { return createPolygon(type, new Vector2(x, y), Vector2.Zero, 0.0f, friction, 1.0f, vertices); }
        public RigidBody2D createPolygon(BodyType type, float x, float y, params Vector2[] vertices) { return createPolygon(type, new Vector2(x, y), Vector2.Zero, 0.0f, 0.2f, 1.0f, vertices); }
        public RigidBody2D createPolygon(BodyType type, float x, float y, float friction, float density, params Vector2[] vertices) { return createPolygon(type, new Vector2(x, y), Vector2.Zero, 0.0f, friction, density, vertices); }
        public RigidBody2D createPolygon(Vector2 position, float friction, params Vector2[] vertices) { return createPolygon(BodyType.DYNAMIC, position, Vector2.Zero, 0.0f, friction, 1.0f, vertices); }
        public RigidBody2D createPolygon(Vector2 position, params Vector2[] vertices) { return createPolygon(BodyType.DYNAMIC, position, Vector2.Zero, 0.0f, 0.2f, 1.0f, vertices); }
        public RigidBody2D createPolygon(Vector2 position, float friction, float density, params Vector2[] vertices) { return createPolygon(BodyType.DYNAMIC, position, Vector2.Zero, 0.0f, friction, density, vertices); }
        public RigidBody2D createPolygon(BodyType type, Vector2 position, float friction, params Vector2[] vertices) { return createPolygon(type, position, Vector2.Zero, 0.0f, friction, 1.0f, vertices); }
        public RigidBody2D createPolygon(BodyType type, Vector2 position, params Vector2[] vertices) { return createPolygon(type, position, Vector2.Zero, 0.0f, 0.2f, 1.0f, vertices); }
        public RigidBody2D createPolygon(BodyType type, Vector2 position, float friction, float density, params Vector2[] vertices) { return createPolygon(type, position, Vector2.Zero, 0.0f, friction, density, vertices); }
        public RigidBody2D createPolygon(BodyType type, Vector2 position, Vector2 center, float orientation, float friction, float density, params Vector2[] vertices)
        {
            PolygonShape shape = new PolygonShape();
            shape.Set(vertices, vertices.Length);

            FixtureDef fd = new FixtureDef();
            fd.shape = shape;
            fd.density = density;
            fd.friction = friction;

            BodyDef bd = new BodyDef();
            bd.type = type;
            bd.position = position;

            Body body = world.CreateBody(bd);
            body.CreateFixture(fd);

            RigidBody2D rb = new RigidBody2D(body);
            managedBodies.Add(rb);
            return rb;
        }

        #endregion

        #region createBody
        public RigidBody2D createBody(Vector2 position) { return createBody(BodyType.STATIC, position.X, position.Y, 0.0f); }
        public RigidBody2D createBody(Vector2 position, float angle) { return createBody(BodyType.STATIC, position.X, position.Y, angle); }
        public RigidBody2D createBody(float x, float y) { return createBody(BodyType.STATIC, x, y, 0.0f); }
        public RigidBody2D createBody(float x, float y, float angle) { return createBody(BodyType.STATIC, x, y, angle); }
        public RigidBody2D createBody(BodyType type, Vector2 position) { return createBody(type, position.X, position.Y, 0.0f); }
        public RigidBody2D createBody(BodyType type, Vector2 position, float angle) { return createBody(type, position.X, position.Y, angle); }
        public RigidBody2D createBody(BodyType type, float x, float y) { return createBody(type, x, y, 0.0f); }
        public RigidBody2D createBody(BodyType type, float x, float y, float angle)
        {
            BodyDef bd = new BodyDef();
            bd.type = type;
            bd.position.X = x;
            bd.position.Y = y;
            bd.angle = angle;

            RigidBody2D rb = new RigidBody2D(world.CreateBody(bd));
            managedBodies.Add(rb);
            return rb;
        }

        #endregion

        #region createCircle
        public RigidBody2D createCircle(float x, float y, float radius, float friction) { return createCircle(BodyType.DYNAMIC, new Vector2(x, y), radius, friction, 1.0f); }
        public RigidBody2D createCircle(float x, float y, float radius) { return createCircle(BodyType.DYNAMIC, new Vector2(x, y), radius, 0.2f, 1.0f); }
        public RigidBody2D createCircle(float x, float y, float radius, float friction, float density) { return createCircle(BodyType.DYNAMIC, new Vector2(x, y), radius, friction, density); }
        public RigidBody2D createCircle(BodyType type, float x, float y, float radius, float friction) { return createCircle(type, new Vector2(x, y), radius, friction, 1.0f); }
        public RigidBody2D createCircle(BodyType type, float x, float y, float radius) { return createCircle(type, new Vector2(x, y), radius, 0.2f, 1.0f); }
        public RigidBody2D createCircle(BodyType type, float x, float y, float radius, float friction, float density) { return createCircle(BodyType.DYNAMIC, new Vector2(x, y), radius, friction, density); }
        public RigidBody2D createCircle(Vector2 position, float radius, float friction) { return createCircle(BodyType.DYNAMIC, position, radius, friction, 1.0f); }
        public RigidBody2D createCircle(Vector2 position, float radius) { return createCircle(BodyType.DYNAMIC, position, radius, 0.2f, 1.0f); }
        public RigidBody2D createCircle(Vector2 position, float radius, float friction, float density) { return createCircle(BodyType.DYNAMIC, position, radius, friction, density); }
        public RigidBody2D createCircle(BodyType type, Vector2 position, float radius, float friction) { return createCircle(type, position, radius, friction, 1.0f); }
        public RigidBody2D createCircle(BodyType type, Vector2 position, float radius) { return createCircle(type, position, radius, 0.2f, 1.0f); }
        public RigidBody2D createCircle(BodyType type, Vector2 position, float radius, float friction, float density)
        {
            CircleShape shape = new CircleShape();
            shape.m_radius = radius;

            FixtureDef fd = new FixtureDef();
            fd.shape = shape;
            fd.density = density;
            fd.friction = friction;

            BodyDef bd = new BodyDef();
            bd.type = type;
            bd.position = position;

            Body body = world.CreateBody(bd);
            body.CreateFixture(fd);

            RigidBody2D rb = new RigidBody2D(body);
            managedBodies.Add(rb);
            return rb;
        }
        #endregion

        #region createEdge
        public RigidBody2D createEdge(Vector2 start, Vector2 end)
        {
            EdgeShape shape = new EdgeShape();
            shape.Set(start, end);

            FixtureDef fd = new FixtureDef();
            fd.shape = shape;

            BodyDef bd = new BodyDef();

            Body body = world.CreateBody(bd);
            body.CreateFixture(fd);

            RigidBody2D rb = new RigidBody2D(body);
            managedBodies.Add(rb);

            return rb;
        }

        #endregion

        #region createDistanceJoint
        public DistanceJoint2D createDistanceJoint(RigidBody2D bodyA, RigidBody2D bodyB)
        {
            return new DistanceJoint2D(bodyA, bodyB);

            //DistanceJointDef jd = new DistanceJointDef();
            ////Vector2 p1, p2, d;

            ////jd.frequencyHz = 4.0f;
            ////jd.dampingRatio = 0.5f;

            //jd.body1 = bodyA.Body;
            //jd.body2 = bodyB.Body;
            ////jd.localAnchor1 = bodyA.Position;
            ////jd.localAnchor2 = bodyB.Position;
            ////p1 = jd.body1.GetWorldPoint(ref jd.localAnchor1);
            ////p2 = jd.body2.GetWorldPoint(ref jd.localAnchor2);
            ////d = p2 - p1;
            //jd.length = (bodyA.Position - bodyB.Position).Length();
            
            //return (DistanceJoint)world.CreateJoint(jd);
        }

        public DistanceJoint createDistanceJoint(RigidBody2D bodyA, Vector2 localAnchorA, RigidBody2D bodyB, Vector2 localAnchorB)
        {
            DistanceJointDef jd = new DistanceJointDef();

            jd.frequencyHz = 4.0f;
            jd.dampingRatio = 0.5f;
            
            jd.body1 = bodyA.Body;
            jd.body2 = bodyB.Body;

            jd.localAnchor1 = localAnchorA;
            jd.localAnchor2 = localAnchorB;

            Vector2 p1 = jd.body1.GetWorldPoint(ref jd.localAnchor1);
            Vector2 p2 = jd.body2.GetWorldPoint(ref jd.localAnchor2);
            Vector2 d = p2 - p1;
            jd.length = d.Length();

            return (DistanceJoint)world.CreateJoint(jd);
        }

        #endregion

        #region createRevoluteJoint
        public RevoluteJoint createRevoluteJoint(RigidBody2D bodyA, RigidBody2D bodyB) { return createRevoluteJoint(bodyA, Vector2.Zero, bodyB, new Vector2(0.0f, 10.0f), false); }
        public RevoluteJoint createRevoluteJoint(RigidBody2D bodyA, RigidBody2D bodyB, Vector2 localAnchor) { return createRevoluteJoint(bodyA, localAnchor, bodyB, localAnchor, false); }
        public RevoluteJoint createRevoluteJoint(RigidBody2D bodyA, RigidBody2D bodyB, Vector2 localAnchor, bool collideConnected) { return createRevoluteJoint(bodyA, localAnchor, bodyB, localAnchor, collideConnected); }
        public RevoluteJoint createRevoluteJoint(RigidBody2D bodyA, Vector2 localAnchor1, RigidBody2D bodyB, Vector2 localAnchor2) { return createRevoluteJoint(bodyA, localAnchor1, bodyB, localAnchor2, false); }
        public RevoluteJoint createRevoluteJoint(RigidBody2D bodyA, Vector2 localAnchor1, RigidBody2D bodyB, Vector2 localAnchor2, bool collideConnected)
        {
            RevoluteJointDef jd = new RevoluteJointDef();
            jd.body1 = bodyA.Body;
            jd.body2 = bodyB.Body;

            jd.localAnchor1 = bodyA.Body.GetLocalPoint(localAnchor1);
            jd.localAnchor2 = bodyB.Body.GetLocalPoint(localAnchor2);

            jd.collideConnected = collideConnected;
            jd.enableLimit = false;

            //jd.enableMotor = true;
            //jd.maxMotorTorque = 1000.0f;
            //jd.enableLimit = true;

            //jd.motorSpeed = 0.0f;
            //jd.localAnchor1 = p1;
            //jd.lowerAngle = -30.0f * (float)System.Math.PI / 180.0f;
            //jd.upperAngle = 5.0f * (float)System.Math.PI / 180.0f;
            return (RevoluteJoint)world.CreateJoint(jd);
        }

        #endregion

        #region createPrismaticJoint
        public PrismaticJoint createPrismaticJoint(RigidBody2D bodyA, RigidBody2D bodyB) { return createPrismaticJoint(bodyA, bodyB, Vector2.Zero, Vector2.UnitX); }
        public PrismaticJoint createPrismaticJoint(RigidBody2D bodyA, RigidBody2D bodyB, Vector2 localAnchor, Vector2 axis)
        {
            PrismaticJointDef pd = new PrismaticJointDef();
            pd.body1 = bodyA.Body;
            pd.body2 = bodyB.Body;

            pd.collideConnected = false;
            pd.localAxis1 = bodyA.Body.GetLocalVector(axis);

            pd.referenceAngle = bodyB.Angle - bodyA.Angle;

            pd.enableLimit = true;
            pd.lowerTranslation = 0.0f;
            pd.upperTranslation = -0.0f;

            pd.localAnchor1 = bodyA.Body.GetLocalPoint(localAnchor);
            pd.localAnchor2 = bodyB.Body.GetLocalPoint(localAnchor);

            return (PrismaticJoint)world.CreateJoint(pd);
        }

        public PrismaticJoint createPrismaticJoint(RigidBody2D bodyA, RigidBody2D bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis)
        {
            PrismaticJointDef pd = new PrismaticJointDef();
            pd.body1 = bodyA.Body;
            pd.body2 = bodyB.Body;

            pd.collideConnected = false;
            pd.localAxis1 = axis;

            pd.localAnchor1 = localAnchorA;
            pd.localAnchor2 = localAnchorB;

            return (PrismaticJoint)world.CreateJoint(pd);
        }
        #endregion

        #region createLineJoint
        public LineJoint createLineJoint(RigidBody2D bodyA, Vector2 localAnchorA, RigidBody2D bodyB, Vector2 localAnchorB)
        {
            Vector2 worldAxis = new Vector2(0.0f, 1.0f);

            LineJointDef pd = new LineJointDef();
            pd.body1 = bodyA.Body;
            pd.body2 = bodyB.Body;

            pd.collideConnected = false;
            pd.localAxis1 = worldAxis;

            //pd.enableLimit = true;
            //pd.lowerTranslation = 0.0f;
            //pd.upperTranslation = -1.0f;

            pd.localAnchor1 = localAnchorA;
            pd.localAnchor2 = localAnchorB;

            return (LineJoint)world.CreateJoint(pd);
        }

        #endregion

        #region createWeldJoint
        public WeldJoint createWeldJoint(RigidBody2D bodyA, RigidBody2D bodyB, Vector2 anchor)
        {
            WeldJointDef jd = new WeldJointDef();
            jd.Initialize(bodyA.Body, bodyB.Body, anchor);
            return (WeldJoint)world.CreateJoint(jd);
        }

        #endregion

        #region destroyBody

        /// <summary>
        /// Destroys a body. This method is similar to the method of world but this one 
        /// queues bodies for destruction if the world is locked during the destruction 
        /// call. The world method does nothing in this case.
        /// </summary>
        /// <param name="body">The body to be destroyed.</param>
        /// <remarks>
        /// All bodies that you try to destroy during a lock phase of the world are destroyed 
        /// immediately after the next world unlocking.
        /// </remarks>
        public void destroyBody(Body body)
        {
            if (world.IsLocked())
            {
                // queue body for destruction
                bodiesToDestroy.Add(body);
            }
            else
            {
                world.DestroyBody(body);
            }
        }

        public void destroyBody(RigidBody2D body) 
        { 
            destroyBody(body.Body);
            managedBodies.Remove(body);
        }

        #endregion

        #region destroyJoint
        public void destroyJoint(Joint joint)
        {
            if (world.IsLocked())
            {
                // queue joint for destruction
                jointsToDestroy.Add(joint);
            }
            else
            {
                world.DestroyJoint(joint);
            }
        }
        public void destroyJoint(Joint2D joint) 
        { 
            destroyJoint(joint.Joint);
            managedJoints.Remove(joint);
        }

        #endregion

        #endregion

        void world_WorldUnlock(object sender, EventArgs e)
        {
            if (bodiesToDestroy.Count > 0)
            {
                foreach (Body b in bodiesToDestroy)
                {
                    world.DestroyBody(b);
                }

                bodiesToDestroy.Clear();
            }

            if (jointsToDestroy.Count > 0)
            {
                foreach (Joint j in jointsToDestroy)
                {
                    world.DestroyJoint(j);
                }

                jointsToDestroy.Clear();
            }
        }

        public void AddController(Controller2D controller)
        {
            controllers.Add(controller);
            if (controllers.Count > 0)
            {
                // enable pre solve event handling
                world.PreSolve += new PreSolveEventHandler(world_PreSolve);
            }
        }

        void world_PreSolve(Object sender, TimeStep dt)
        {
            if (enabledControllers <= 0) return;    // nothing to do

            enabledControllers = 0;                 // reset
            foreach (Controller2D c in controllers)
            {
                if (c.Enabled)
                {
                    c.Update(dt);
                    enabledControllers++;
                }
            }
        }

        public void RemoveController(Controller2D controller)
        {
            controllers.Remove(controller);
            if (controllers.Count <= 0)
            {
                // disable pre solve event handling
                world.PreSolve -= new PreSolveEventHandler(world_PreSolve);
            }
        }
    }
}
